function P  = Lsq( q, x, nCam, nPoints, f0, n0, c0, w0 )
    p = x(1:nPoints , 1:3);
    f = x(1:nCam , 4);
    n = x(1:nCam, 5:7);
    c = x(1:nCam, 8:10);
    w = x(1:nCam, 11);
    P = LsqInt(q,p,f,n,c,w,nCam,nPoints,f0,n0,c0,w0);
end

function P  = LsqInt( q, p, f, n, c, w, nCam, nPoints, f0, n0, c0, w0 )
    P = 0;
    %first cam is already known
    camStride = nPoints*2;
    pointStride = 2;
    for i = 1:(nCam + 1)
        if ( i == 1)
            currNormal = n0;
            currFocal = f0;
            currAngle = w0;
            currCenter = c0;
        else
            currNormal = n(i - 1, 1:3);
            currFocal = f(i - 1);
            currAngle = w(i - 1);
            currCenter = c(i - 1, 1:3);
        end
        
        for j = 1:nPoints
            currProj = q(i, 2*j - 1 : 2*j);
            currEstimatedProj = p(j , 1 :3);
            P_pSpace = project(currFocal, currNormal, currCenter, currAngle, currEstimatedProj) ;
            %screen space transform
            ImageDim = [320, 240];  
            P_sSpace = (P_pSpace.*ImageDim) + ImageDim;
            temp = currProj - P_sSpace;
            currLoc = (i - 1) * camStride + (j - 1) * pointStride;
            P(currLoc + 1) = temp(1);
            P(currLoc + 2) = temp(2);
        end
    end
end

function P = project(f, n, c, w, p)
    d = [f f 1];
    K = diag(d);
    I = eye(3,3);
    nNormalized = normr(n);
    nCross = [0, -nNormalized(3), nNormalized(2) ; nNormalized(3), 0, -nNormalized(1) ; -nNormalized(2), nNormalized(1), 0 ];
    R = cos(w)*I + sin(w)*nCross + (1-cos(w))*(n'*n);
    P_tag = K*R*(p-c)';
    P = [P_tag(1)/P_tag(3), P_tag(2)/P_tag(3)];
end

